#include "motor_mg513.h"
extern TIM_HandleTypeDef htim5;
extern int jiaqu[2];
int32_t uint32_to_int32(uint32_t value) {
    return (int32_t)value;
}
int16_t Mg513_Output_Calculate(Mg513_Pid *mg513pid,float target)
{
	mg513pid->ref=target;
	mg513pid->fdb=uint32_to_int32(__HAL_TIM_GET_COUNTER(&htim5));
	mg513pid->error[0]=(float)(mg513pid->ref- mg513pid->fdb) / 20000;
	if(mg513pid->integral>(-mg513pid->outputmax / 2)&&mg513pid->integral<(mg513pid->outputmax / 2))
	{
		mg513pid->integral += mg513pid->error[0];
	}
	mg513pid->output=mg513pid->kp*mg513pid->error[0]+
									 mg513pid->ki*mg513pid->integral+
									 mg513pid->kd*(mg513pid->error[0]-mg513pid->error[1]);
	mg513pid->error[1]=mg513pid->error[0];
	mg513pid->error[2]=mg513pid->error[1];	
//	if(mg513pid->output<0)
//	{
//		if(mg513pid->output< -mg513pid->outputmax)	return (int16_t)(-mg513pid->outputmax);
//		if(mg513pid->output> -mg513pid->outputmin)	return (int16_t)(-mg513pid->outputmin);
//		return (int16_t)mg513pid->output;
//	}
//	if(mg513pid->output>0)
//	{
//		if(mg513pid->output> mg513pid->outputmax)	return (int16_t)(mg513pid->outputmax);
//		if(mg513pid->output< mg513pid->outputmin)	return (int16_t)(mg513pid->outputmin);
//		return (int16_t)mg513pid->output;
//	}	
	if(mg513pid->output<0)
	{
		if(mg513pid->output< -500)	mg513pid->output=-500;
		if(mg513pid->output> -200)		mg513pid->output=-200;
		return (int16_t)mg513pid->output;
	}
	if(mg513pid->output>0)
	{
		if(mg513pid->output> 998) mg513pid->output=998;
		if(mg513pid->output< 650)	mg513pid->output=650;
		return (int16_t)mg513pid->output;
	}		
}	
void Mg513Run(Mg513_Pid *mg513pid,float target)
{
	int16_t output=Mg513_Output_Calculate(mg513pid,target);
	if(output>0)
	{
		HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 1);
		HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 0); 	
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, output);
	}
	else
	{
		HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 0);
		HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 1); 	
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, -output);
	}	
}
void Mg513Run_up(Mg513_Pid *mg513pid,float target)
{
	int16_t output=Mg513_Output_Calculate(mg513pid,target);
	if(output>0)
	{
		HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 1);
		HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 0); 	
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 998);
	}
	else
	{
		HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 0);
		HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 1); 	
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, -output);
	}	
}
void SetMg513Pid(Mg513_Pid *mg513pid,float kp,float ki,float kd,float outputmin,float outputmax)
{
	mg513pid->kp=kp;
	mg513pid->ki=ki;
	mg513pid->kd=kd;
	mg513pid->outputmin=outputmin;
	mg513pid->outputmax=outputmax;
	mg513pid->fdb=0;
	mg513pid->ref=0;
	mg513pid->integral=0;
	mg513pid->output=0;
	mg513pid->error[0]=0;
	mg513pid->error[1]=0;
	mg513pid->error[2]=0;	
}	
void zhuanzi1heshang(float Z_position,Mg513_Pid *mg513pid)
{
    uint16_t i = 0;
    while (i < 2500)
    {
        Mg513Run(mg513pid, Z_position);   
        HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0); 
        HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 1); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 998);	
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	jiaqu[0]=1;
}
void zhuanzi1songkai(float Z_position,Mg513_Pid *mg513pid)
{
    uint16_t i = 0;
    while (i < 2500)
    {
        Mg513Run(mg513pid, Z_position);   
        HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 1); 
        HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 998);	
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	jiaqu[0]=0;	
}
void zhuanzi2heshang(float Z_position,Mg513_Pid *mg513pid)
{
    uint16_t i = 0;
    while (i < 2500)
    {
        Mg513Run(mg513pid, Z_position);   
        HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0); 
        HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 1); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 998);	
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	jiaqu[1]=1;	
}
void zhuanzi2songkai(float Z_position,Mg513_Pid *mg513pid)
{
    uint16_t i = 0;
    while (i < 2500)
    {
        Mg513Run(mg513pid, Z_position);   
        HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 1); 
        HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 998);	
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	jiaqu[1]=0;	
}
